#ifndef _CHASSIS_CTRL_H_
#define _CHASSIS_CTRL_H_

#include <chrono>
#include <cstdio>
#include <memory>
#include <utility>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include <sensor_msgs/msg/imu.hpp>
#include "robot_msgs/msg/vi_yaw.hpp"
#include "angles/angles.h"

class ChassisCtrl
{
public:
    ChassisCtrl();
    ~ChassisCtrl();
    void work_loop();
    void ImuCallback(const sensor_msgs::msg::Imu::SharedPtr vel_cmd);
    void ViyawCallback(const robot_msgs::msg::ViYaw::SharedPtr msg);
    double CalcValue(std::vector<double> data);
private:
    rclcpp::Node::SharedPtr node;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    rclcpp::Subscription<robot_msgs::msg::ViYaw>::SharedPtr viyaw_sub_;
    size_t count_ = 0;
    std::vector<double> x_data;
    std::vector<double> y_data;
};

#endif